﻿#include "TrueRoundness.h"

TrueRoundness::TrueRoundness(unsigned int top, unsigned int bottom)
{
    m_type = 0;
    m_top = top;
    m_bottom = bottom;
}

TrueRoundness::TrueRoundness(double top_percent, double bottom_percent)
{
    m_type = 1;
    m_top_percent = top_percent;
    m_bottom_percent = bottom_percent;
}

double TrueRoundness::operator()(const std::vector<cv::Point2i> &contour, double &r_min, double &r_max)
{
    double center_x;
    double center_y;
    double radius;
    bool ret = circleLeastFit(contour, center_x, center_y, radius);
    if(!ret) return -1;

    std::vector<cv::Point2i> newcontour = contour;
    sort(newcontour, center_x, center_y);

    int N = newcontour.size();
    if(m_type == 0)
    {
        if(m_top > N || m_bottom > N)
        {
            return -2;
        }
        int N1 = m_top;
        int N2 = N - 1 - m_bottom;
        if(distance(newcontour[N1], center_x, center_y) < radius || distance(newcontour[N2], center_x, center_y) > radius)
        {
            return -3;
        }

        r_max = distance(newcontour[N1], center_x, center_y);
        r_min = distance(newcontour[N2], center_x, center_y);
        return  r_max - r_min ;
    }
    else if(m_type == 1)
    {
        if(m_top_percent >= 1 || m_bottom_percent >= 1) return -4;
        int N1 = 0, N2 = 0;
        for(int i = 0; i < N; i++)
        {
            double r = distance(newcontour[i], center_x, center_y);
            if(r > radius) N1 ++;
            if(r < radius) N2 ++;
        }
        N1 = N1 * (m_top_percent);
        N2 = N - 1 - N2 * (m_bottom_percent);
        r_max = distance(newcontour[N1], center_x, center_y);
        r_min = distance(newcontour[N2], center_x, center_y);
        return  r_max - r_min ;
    }

    return -5;
}

double TrueRoundness::operator()(const std::vector<cv::Point2f> &contour, double &r_min, double &r_max)
{
    double center_x;
    double center_y;
    double radius;
    bool ret = circleLeastFit(contour, center_x, center_y, radius);
    if(!ret) return -1;

    std::vector<cv::Point2f> newcontour = contour;
    sort(newcontour, center_x, center_y);

    int N = newcontour.size();
    if(m_type == 0)
    {
        if(m_top > N || m_bottom > N)
        {
            return -2;
        }
        int N1 = m_top;
        int N2 = N - 1 - m_bottom;
        if(distance(newcontour[N1], center_x, center_y) < radius || distance(newcontour[N2], center_x, center_y) > radius)
        {
            return -3;
        }

        r_max = distance(newcontour[N1], center_x, center_y);
        r_min = distance(newcontour[N2], center_x, center_y);
        return  r_max - r_min ;
    }
    else if(m_type == 1)
    {
        if(m_top_percent >= 1 || m_bottom_percent >= 1) return -4;
        int N1 = 0, N2 = 0;
        for(int i = 0; i < N; i++)
        {
            double r = distance(newcontour[i], center_x, center_y);
            if(r > radius) N1 ++;
            if(r < radius) N2 ++;
        }
        N1 = N1 * (m_top_percent);
        N2 = N - 1 - N2 * (m_bottom_percent);
        r_max = distance(newcontour[N1], center_x, center_y);
        r_min = distance(newcontour[N2], center_x, center_y);
        return  r_max - r_min ;
    }

    return -5;
}

bool TrueRoundness::circleLeastFit(const std::vector<cv::Point2i> &points, double &center_x, double &center_y, double &radius)
{
    center_x = 0.0;
    center_y = 0.0;
    radius = 0.0;
    if (points.size() < 3)
    {
        return false;
    }

    double sum_x = 0.0, sum_y = 0.0;
    double sum_x2 = 0.0, sum_y2 = 0.0;
    double sum_x3 = 0.0, sum_y3 = 0.0;
    double sum_xy = 0.0, sum_x1y2 = 0.0, sum_x2y1 = 0.0;

    int N = points.size();
    for (int i = 0; i < N; i++)
    {
        double x = points[i].x;
        double y = points[i].y;
        double x2 = x * x;
        double y2 = y * y;
        sum_x += x;
        sum_y += y;
        sum_x2 += x2;
        sum_y2 += y2;
        sum_x3 += x2 * x;
        sum_y3 += y2 * y;
        sum_xy += x * y;
        sum_x1y2 += x * y2;
        sum_x2y1 += x2 * y;
    }

    double C, D, E, G, H;
    double a, b, c;

    C = N * sum_x2 - sum_x * sum_x;
    D = N * sum_xy - sum_x * sum_y;
    E = N * sum_x3 + N * sum_x1y2 - (sum_x2 + sum_y2) * sum_x;
    G = N * sum_y2 - sum_y * sum_y;
    H = N * sum_x2y1 + N * sum_y3 - (sum_x2 + sum_y2) * sum_y;
    a = (H * D - E * G) / (C * G - D * D);
    b = (H * C - E * D) / (D * D - G * C);
    c = -(a * sum_x + b * sum_y + sum_x2 + sum_y2) / N;

    center_x = a / (-2);
    center_y = b / (-2);
    radius = sqrt(a * a + b * b - 4 * c) / 2;
    return true;
}

bool TrueRoundness::circleLeastFit(const std::vector<cv::Point2f> &points, double &center_x, double &center_y, double &radius)
{
    center_x = 0.0;
    center_y = 0.0;
    radius = 0.0;
    if (points.size() < 3)
    {
        return false;
    }

    double sum_x = 0.0, sum_y = 0.0;
    double sum_x2 = 0.0, sum_y2 = 0.0;
    double sum_x3 = 0.0, sum_y3 = 0.0;
    double sum_xy = 0.0, sum_x1y2 = 0.0, sum_x2y1 = 0.0;

    int N = points.size();
    for (int i = 0; i < N; i++)
    {
        double x = points[i].x;
        double y = points[i].y;
        double x2 = x * x;
        double y2 = y * y;
        sum_x += x;
        sum_y += y;
        sum_x2 += x2;
        sum_y2 += y2;
        sum_x3 += x2 * x;
        sum_y3 += y2 * y;
        sum_xy += x * y;
        sum_x1y2 += x * y2;
        sum_x2y1 += x2 * y;
    }

    double C, D, E, G, H;
    double a, b, c;

    C = N * sum_x2 - sum_x * sum_x;
    D = N * sum_xy - sum_x * sum_y;
    E = N * sum_x3 + N * sum_x1y2 - (sum_x2 + sum_y2) * sum_x;
    G = N * sum_y2 - sum_y * sum_y;
    H = N * sum_x2y1 + N * sum_y3 - (sum_x2 + sum_y2) * sum_y;
    a = (H * D - E * G) / (C * G - D * D);
    b = (H * C - E * D) / (D * D - G * C);
    c = -(a * sum_x + b * sum_y + sum_x2 + sum_y2) / N;

    center_x = a / (-2);
    center_y = b / (-2);
    radius = sqrt(a * a + b * b - 4 * c) / 2;
    return true;
}

void TrueRoundness::sort(std::vector<cv::Point2i> &contour, double center_x, double center_y)
{
    auto cmp = [center_x, center_y](cv::Point2i p1, cv::Point2i p2)
    {double r1 = hypot(p1.x - center_x, p1.y - center_y);
     double r2 = hypot(p2.x - center_x, p2.y - center_y);
     return r1 > r2;};

    std::sort( contour.begin(), contour.end(), cmp );

}

void TrueRoundness::sort(std::vector<cv::Point2f> &contour, double center_x, double center_y)
{
    auto cmp = [center_x, center_y](cv::Point2i p1, cv::Point2i p2)
    {double r1 = hypot(p1.x - center_x, p1.y - center_y);
     double r2 = hypot(p2.x - center_x, p2.y - center_y);
     return r1 > r2;};

    std::sort( contour.begin(), contour.end(), cmp );
}

double TrueRoundness::distance(cv::Point2i p, double center_x, double center_y)
{
    return hypot(p.x - center_x, p.y - center_y);
}

double TrueRoundness::distance(cv::Point2f p, double center_x, double center_y)
{
    return hypot(p.x - center_x, p.y - center_y);
}
